%**************************************************************************
%BatBot: Biological inspired Bat roBot.

%Copyright RObotics and Cybernetics Group
%Julian Colorado

% Matlab simulator of bat flight behavior. 
%**************************************************************************

%O(n) Force Equations of motion for Rigid-Body-Dynamics
%This program returns effective Forces to generate locomotion

%Entrance parameters:

%n: number of robots within the MRS.
%DHC:   alpha  a teta  d  sigma  m  Ixx  Iyy  Izz  sx  sy  sz
%Joint Trajectory:  Q,dQ,d2Q  of nxm, where n=# of trajectory points and m=#degrees of freedom   
%6-dimensional External Force: Fext
%6-dimensional gravity vector: GRAV

function [T,KT] = inv_dyn_JDC(n,DHC,Q,dQ,d2Q,Fext,GRAV)

[pt,dof]=size(Q); %# of trajectory points to compute
K = 0;  %Energy cost initialized to zero.
PL = 0;
Vcm = 0;

%Compute Inverse Dynamics for each point of the trajectory
for k=1:pt
    
    %a virtual base fixed robot is addressed in order to use it as a frame of reference and
    %compute dynamics for floating base systems
    V(:,1)=[0;0;0;0;0;0];
    dV(:,1)=GRAV;
    
    
    %Backward Recurrence Propagating Kinematic parameters: Spatial Velocity and Acceleration
    for i=1:n
            
        if i==1
            r=eye(3);
            R=[r,zeros(3,3);zeros(3,3),r];
            p=zeros(3,3);
            P=[eye(3),-p;zeros(3,3),eye(3)];
        else
            %Calculating Homogeneous transformation matrix
            teta=Q(k,i);
            rot=fkine2(DHC(i,1:5),teta);
            r=rot(1:3,1:3);
            p=[-DHC(i,2);-DHC(i,4)*sin(DHC(i,1));-DHC(i,4)*cos(DHC(i,1))];
            psk=[0 -p(3),p(2);p(3),0,-p(1);-p(2),p(1),0];
            R=[r,zeros(3,3);zeros(3,3),r];
            P=[eye(3),-psk;zeros(3,3),eye(3)];
        end
        
        %Evaluating if virtual joint is revolute or prismatic
        if DHC(i,5)==0
            H=[0;0;1;0;0;0];
        else
            H=[0;0;0;0;0;1];
        end
        
         %Compute Spatial Velocity
        V(:,i+1)=P'*R'*V(:,i)+H*dQ(k,i);
      
        wsk=[0 -V(3,i+1),V(2,i+1);V(3,i+1),0,-V(1,i+1);-V(2,i+1),V(1,i+1),0];  %actual
        wsk6=[wsk,zeros(3,3);zeros(3,3),wsk]; 
        dH=wsk6*H;
           
        wa=[V(1,i);V(2,i);V(3,i)];
        wa=r'*wa;
        wska=[0 -wa(3),wa(2);wa(3),0,-wa(1);-wa(2),wa(1),0]; 
        wska6=[wska,zeros(3,3);zeros(3,3),wska];
        dP=wska6*P';
       
        %Compute Spatial Acceleration          
        dV(:,i+1)=P'*R'*dV(:,i)+H*d2Q(k,i)...
            +[zeros(3,1);wska*(V(4:6,i+1)-r'*V(4:6,i))]+...
            +[wsk zeros(3,3);zeros(3,3) wsk]*H*dQ(k,i);
                       
    end    
    
    %Backward Recurrence Propagating Dynamics parameters
    F(:,n+1)=Fext;
    for i=n:-1:1
        
        %Calculating Homogeneous transformation matrix
        teta=Q(k,i);
        rot=fkine2(DHC(i,1:5),teta);
        r=rot(1:3,1:3);
        p=[-DHC(i,2);-DHC(i,4)*sin(DHC(i,1));-DHC(i,4)*cos(DHC(i,1))];
        psk=[0 -p(3),p(2);p(3),0,-p(1);-p(2),p(1),0];
        R=[r,zeros(3,3);zeros(3,3),r];
        P=[eye(3),-psk;zeros(3,3),eye(3)];
        s=DHC(i,10:12);
        s1=r*(s'-p);  
        ssk=[0 -s1(3),s1(2);s1(3),0,-s1(1);-s1(2),s1(1),0];  
        S=[eye(3) ssk;zeros(3,3) eye(3)];
        wsk=[0 -V(3,i+1),V(2,i+1);V(3,i+1),0,-V(1,i+1);-V(2,i+1),V(1,i+1),0];
        dS=[zeros(3,3),wsk*ssk;zeros(3,3),zeros(3,3)];
        
        %calculating 6-dimensional Inertial operator
        Ixx=DHC(i,7);
        Iyy=DHC(i,8);
        Izz=DHC(i,9);
        Jcm=[Ixx 0 0;0 Iyy 0;0 0 Izz];
        Icm=[Jcm zeros(3,3);zeros(3,3) DHC(i,6)*eye(3)];
        I=S*Icm*S';
        dI=[wsk*Jcm,zeros(3,3);zeros(3,3),zeros(3,3)];
         
        
        
        %Compute Spatial forces
        %F(:,i)=R*P*F(:,i+1)+I*dV(:,i+1)+(dI+I*dS')*V(:,i+1);
        F(:,i)=R*P*F(:,i+1)+I*dV(:,i+1)+[wsk*I(1:3,1:3)*V(1:3,i+1);DHC(i,6)*wsk*wsk*s1];
        
        
        %Projecting F in the axe of motion using H
        if DHC(i,5)==0
            H=[0;0;1;0;0;0];
        else
            H=[0;0;0;0;0;1]; 
        end
        
        T(k,i)=H'*F(:,i);
               
%         
% % %         %Kinetic Enery: Add local components of energy per body
%          K(k,(n+1)-i) = 0.5*V(:,i+1)'*I*V(:,i+1));
% %         %Power loss due to Friction         
%          PL(k,(n+1)-i) = [V(4,i+1) V(5,i+1)]*([-10*DHC(i,6)*(DHC(i,10)^2)/3  0; 0 0]+...
%                          [cos(teta) -sin(teta);sin(teta) cos(teta)]*[0.1*DHC(i,6) 0;0 10*DHC(i,6)]*[cos(teta) -sin(teta);sin(teta) cos(teta)]')*...
%                          [V(4,i+1);V(5,i+1)];
% 
         K = K+(0.5*V(:,i+1)'*I*V(:,i+1));
%         PL = PL+([V(4,i+1) V(5,i+1)]*([-10*DHC(i,6)*(DHC(i,10)^2)/3  0; 0 0]+...
%                           [cos(teta) -sin(teta);sin(teta) cos(teta)]*[0.1*DHC(i,6) 0;0 10*DHC(i,6)]*[cos(teta) -sin(teta);sin(teta) cos(teta)]')*...
%                           [V(4,i+1);V(5,i+1)]);
%         Vcm = sqrt([V(4,i+1) V(5,i+1)]*[V(4,i+1);V(5,i+1)])+Vcm;           
                      
    end
    %Stacking Energy body-contribution per trajectory point
    KT(k) = K;
%     PLT(k) = (PL/n)*0.01;
%     VT(k) = (Vcm*0.27)/n;
    
  end
  
end



